#include <gtest/gtest.h>

#include "rtdev/safetyboard/scbitf.h"
#include "rtdev/safetyboard/hscb/hscb.h"
#include "hwdrv/hwdrv.h"
#include "rtbus/rtbus.h"
#include "jkutil/timespec.h"

using rtdev::HScb;
using rtdev::SafetyBoardItf;

class HScbTest : public ::testing::Test
{
public:
    virtual void SetUp() override { hscb = SafetyBoardItf::create(rtdev::kScbTypeMaxcab); }
    virtual void TearDown() override { delete hscb; }

    rtdev::SafetyBoardItf* hscb = nullptr;
};

//测试HScb的初始化过程
TEST_F(HScbTest, init)
{
    hwdrv::HwMgr::ScbSlotInfo info;
    info.type = rtdev::kScbTypeMaxcab;
    info.devid = 0;
    info.bus.type = "ecat";
    info.bus.interface = "0";
    ASSERT_EQ(hscb->init(&info, 0), 0);
}

// 测试HScb的boot启动过程已经对应的周期通讯
TEST_F(HScbTest, boot)
{
    hwdrv::HwMgr::ScbSlotInfo info;
    info.type = rtdev::kScbTypeMaxcab;
    info.devid = 0;
    info.bus.type = "ecat";
    info.bus.interface = "0";
    ASSERT_EQ(hscb->init(&info, 0), 0);

    ASSERT_EQ(hscb->boot(), 0);
    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    for (int i = 0;; i++)
    {
        int64_t toff = 0;
        rtbus::BusMgr::get()->update(&toff);
        hscb->get_recv_data();
        hscb->set_send_data();

        struct timespec dt;
        dt.tv_sec = 0;
        dt.tv_nsec = 8000 * 1000 + toff;
        next = timespec_add(next, dt);
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
}

// 本体上下电
TEST_F(HScbTest, robpower)
{
    hwdrv::HwMgr::ScbSlotInfo info;
    info.type = rtdev::kScbTypeMaxcab;
    info.devid = 0;
    info.bus.type = "ecat";
    info.bus.interface = "0";
    ASSERT_EQ(hscb->init(&info, 0), 0);

    ASSERT_EQ(hscb->boot(), 0);
    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    for (int i = 0;; i++)
    {
        int64_t toff = 0;
        rtbus::BusMgr::get()->update(&toff);
        hscb->get_recv_data();
        hscb->set_send_data();
        if (i <= 200)
        {
            hscb->rob_power()->set_power(0);
        }
        else if (i <= 1000)
        {
            hscb->rob_power()->set_power(1);  //指令上电
        }
        // else if (i <= 1500)
        // {
        //     //检查电压是否正常
        //     auto vol = hscb->rob_power()->get_voltage(-1);
        //     EXPECT_NE(vol, 0);
        // }
        else if (i <= 6000)
        {
            hscb->rob_power()->set_power(0);  //指令下电
        }
        else if (i <= 6500)
        {
            //检查电压是否关闭
            auto vol = hscb->rob_power()->get_voltage(-1);
            EXPECT_EQ(vol, 0);
            break;
        }
        else
        {
            break;
        }

        struct timespec dt;
        dt.tv_sec = 0;
        dt.tv_nsec = 8000 * 1000 + toff;
        next = timespec_add(next, dt);
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
}